#include "rclcpp/rclcpp.hpp"
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

class ObjectDetector : public rclcpp::Node
{
public:
    ObjectDetector() : Node("object_detector"), logger_(this->get_logger())
    {
        RCLCPP_INFO(logger_, "节点已经启动!");
        subscriber_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/sensor_camera_rgb/image_raw", 1,
            std::bind(&ObjectDetector::camera_rgb_callback, this, std::placeholders::_1));
        cv::namedWindow("RGB");
    }

private:
    void camera_rgb_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
            cv::Mat imgOriginal = cv_ptr->image;
            cv::imshow("RGB", imgOriginal);
            cv::waitKey(1);
        }
        catch (cv_bridge::Exception &e)
        {
            RCLCPP_ERROR(logger_, "cv_bridge exception: %s", e.what());
        }
    }
    rclcpp::Logger logger_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscriber_;
};

int main(int argc, char *argv[])
{
    /* 初始化rclcpp  */
    rclcpp::init(argc, argv);
    /* 产生一个 节点 */
    auto node = std::make_shared<ObjectDetector>();
    /* 运行节点，并检测退出信号 Ctrl+C */
    rclcpp::spin(node);
    cv::destroyAllWindows();
    /* 停止运行 */
    rclcpp::shutdown();
    return 0;
}